#!/usr/bin/env python

import rospy
import time
from gazebo_msgs.msg import ModelState, ModelStates

class Combination():
    def __init__(self):
        self.pub_model = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size=1)
        self.moving()

    def moving(self):
        state = 0
	state1 = 0
        state2 = 0
        state3 = 0
        state4 = 0
        while state == 4 or not rospy.is_shutdown():
            model = rospy.wait_for_message('gazebo/model_states', ModelStates)
            for i in range(len(model.name)):
                if model.name[i] == 'Dynamic_obstacle_1':
                    if state1 == 0:
			obstacle_1 = ModelState()
                        obstacle_1.model_name = model.name[i]
                        obstacle_1.pose = model.pose[i]
                        obstacle_1.pose.position.x += 0.01
                        self.pub_model.publish(obstacle_1)
                        if abs(obstacle_1.pose.position.x - 0.8) < 0.01 and abs(obstacle_1.pose.position.y + 0.8) < 0.01:
                            state1 = 1
			    print('state1')

                elif model.name[i] == 'Dynamic_obstacle_2':
                    if state2 == 0:
                        obstacle_2 = ModelState()
                        obstacle_2.model_name = model.name[i]
                        obstacle_2.pose = model.pose[i]
                        obstacle_2.pose.position.y -= 0.01
                        self.pub_model.publish(obstacle_2)
                        if abs(obstacle_2.pose.position.x + 0.8) < 0.01 and abs(obstacle_2.pose.position.y + 0.8) < 0.01:
                            state2 = 1
			    print('state2')

                elif model.name[i] == 'Dynamic_obstacle_3':
                    if state3 == 0:
                        obstacle_3 = ModelState()
                        obstacle_3.model_name = model.name[i]
                        obstacle_3.pose = model.pose[i]
                        obstacle_3.pose.position.y += 0.01
                        self.pub_model.publish(obstacle_3)
                        if abs(obstacle_3.pose.position.x - 0.8) < 0.01 and abs(obstacle_3.pose.position.y - 0.8) < 0.01:
                            state3 = 1
			    print('state3')

                elif model.name[i] == 'Dynamic_obstacle_4':
                    if state4 == 0:
                        obstacle_4 = ModelState()
                        obstacle_4.model_name = model.name[i]
                        obstacle_4.pose = model.pose[i]
                        obstacle_4.pose.position.x -= 0.01
                        self.pub_model.publish(obstacle_4)
                        if abs(obstacle_4.pose.position.x + 0.8) < 0.01 and abs(obstacle_4.pose.position.y - 0.8) < 0.01:
                            state4 = 1
			    print('state4')
                   
                time.sleep(0.02)
            state = state1 + state2 +state3 + state4

def main():
    rospy.init_node('Dynamic_obstacle')
    try:
        combination = Combination()
    except rospy.ROSInterruptException:
        pass

if __name__ == '__main__':
    main()
